#include "MeBlobDector.h"

#include <stack>
#include <iostream>

#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

using namespace cv;
using namespace std;

// #define DEUBG_BLOB
#define PRINT_TRACE

typedef struct _scanner {
    int m00;
    int m10;
    int m01;
    int width;
    int height;
    int src_step;
    // 简单计算最小最大XY
    int minx, maxx;
    int miny, maxy;
} scanner;


// 显示
static void drawKeyPointsWithCircle(Mat &img1,vector<MeKeyPoint> &keyPoints1)
{
    Scalar color = Scalar(0, 255, 255);
    Mat draw1;
    cvtColor(img1, draw1, COLOR_GRAY2BGR);
    // 将点标主到图上并保存
#ifdef PRINT_TRACE
    cout << "points: ";
#endif
    for (uint i = 0; i < keyPoints1.size(); i++) {
#ifdef PRINT_TRACE
        cout << "[" << keyPoints1[i].pt << "," << keyPoints1[i].area << "," << keyPoints1[i].circularity << "], ";
#endif
        circle(draw1, keyPoints1[i].pt, keyPoints1[i].diameter/2, Scalar(0, 0, 255), 0);
    }
    

#ifdef PRINT_TRACE
    cout << endl;
#endif

    if(keyPoints1.size() != 0) {
        namedWindow("left", 0);
        imshow("left", draw1);
        waitKey();
    }

    draw1.release();
}

static void travel_from(const uchar* origin, const uchar* src, uchar* mask, scanner *ps, int sx, int sy,
                        std::vector<Point> & contours) {
    //printf("Enter travel_from with sx:%d, sy:%d\n", sx, sy);
    std::stack<Point2i> stk;
    stk.push(Point2i(sx, sy));
    contours.push_back(Point2i(sx, sy));
    while(!stk.empty()) {
        Point2i& p = stk.top();
        stk.pop();
        sx = p.x; sy = p.y;
        int offset = ps->src_step * sy;
        int x, lx, rx;
        lx = sx;
        rx = sx;
        // 往左遍历
        for (x = sx - 1; x >= 0; x--) {
            if (src[offset + x] && !mask[offset + x]) {
                lx = x;
                mask[offset + x] = 255;
                ps->m00 += origin[offset + x];
                ps->m10 += origin[offset + x] * x;
                ps->m01 += origin[offset + x] * sy;
            } else {
                break;
            }
        }
        // 往右遍历
        for (x = sx; x < ps->width; x++) {
            if (src[offset + x] && !mask[offset + x]) {
                rx = x;
                mask[offset + x] = 255;
                ps->m00 += origin[offset + x];
                ps->m10 += origin[offset + x] * x;
                ps->m01 += origin[offset + x] * sy;
            } else {
                break;
            }
        }
        // 保存最左，最右
        contours.push_back(Point2i(lx, sy));
        contours.push_back(Point2i(rx, sy));
        if (lx < ps->minx) ps->minx = lx;
        if (rx > ps->maxx) ps->maxx = rx;
        if (sy > ps->maxy) ps->maxy = sy;
        int lastx1 = -2, lastx2 = -2;
        for (x = lx; x <= rx; x++) {
            if (sy > 0 && src[offset - ps->src_step + x] && !mask[offset - ps->src_step + x]) {
                if (x != lastx1 + 1) {
                    stk.push(Point2i(x, sy - 1));
                }
                lastx1 = x;
            }
            if (sy < ps->height-1 && src[offset + ps->src_step + x] && !mask[offset + ps->src_step + x]) {
                if (x != lastx2 + 1) {
                    stk.push(Point2i(x, sy + 1));
                }
                lastx2 = x;
            }
        }
    }

}


int MeBlobDector::fastBlob2DDetect(const Mat &grayImg, const Mat &binaryImg, MeBlobParams &params, int z, std::vector<MeKeyPoint> &keyPoints)
{
    size_t src_step = binaryImg.step;
    Size roi = binaryImg.size();

    Mat tmp = Mat(binaryImg.size(), binaryImg.type());
    uchar* ptmp = tmp.ptr(); // 存放已经搜索过的
    memset(ptmp, 0, roi.width * roi.height);

    //printf("src channels:%d\n", binaryImg.channels());

    const uchar* src = binaryImg.ptr();
    int count = 0;
    scanner scer;
    scer.width = roi.width;
    scer.height = roi.height;
    scer.src_step = src_step;

    double s = 1./255;  // 我们算结果需要乘比例因子

    MeKeyPoint point;
    point.z = z; 
    for(int i = 0; i < roi.height; i++, src += src_step) {
        for (int j = 0; j < roi.width; j++) {
            // 白色 并且 不存在
            if (src[j] > 0 && !ptmp[i * src_step + j]) {

                scer.m00 = 0;
                scer.m10 = 0;
                scer.m01 = 0;
                scer.minx = roi.width; scer.miny = i;
                scer.maxx = 0; scer.maxy = 0;
                float diameter = 0.0;

                std::vector<Point> contours;
                travel_from(grayImg.ptr(), binaryImg.ptr(), ptmp, &scer, j, i, contours);
                if (params.filterByArea)
                {
                    double area = scer.m00*s;
                    if (area < params.minArea || area >= params.maxArea)
                        continue;

                    // 通过面积计算直径
                    diameter = (float)sqrt(2.0*area/CV_PI);
                    point.area = area;
                    point.diameter = diameter;
                }

                // 简单的长宽比
                double ratio = double(min((scer.maxx- scer.minx), (scer.maxy - scer.miny)))/max((scer.maxx- scer.minx), (scer.maxy - scer.miny));;
                point.circularity = ratio;
                if (params.filterByCircularity)
                {
                    if (ratio < params.minCircularity || ratio >= params.maxCircularity)
                        continue;
                }

                if (scer.m00 != 0) {
                    float cx = (float)scer.m10 / (float)scer.m00;
                    float cy = (float)scer.m01 / (float)scer.m00;
                    // diameter = double((scer.maxx- scer.minx) + (scer.maxy - scer.miny))/2.0;
                    //key_points.push_back(Point2f(cx, cy));
                    point.pt = Point2f(cx,cy);
                    keyPoints.push_back(point);
                    count++;
                }

                //printf("after travel_from with (%d, %d)\n", j, i);
            }
        }
    }

    return count;
}



static void travelUp(MeKeyPoint &basePoint, vector<vector<MeKeyPoint>> &keyPoints, vector<vector<char>> &mask,
                     int nextz, int zDim,
                     vector<MeKeyPoint> &outPoints)
{
    outPoints.push_back(basePoint);
    MeKeyPoint nextBasePoint = basePoint;
    for (int nz = nextz; nz < zDim; nz++) {
        int nMatched = 0;
        for (int i = 0; i< keyPoints[nz].size(); i++) {
            if(mask[nz][i] == 1) {
                continue;
            }
            double dis = cv::norm(keyPoints[nz][i].pt - nextBasePoint.pt);
            if (dis <= max(nextBasePoint.diameter, keyPoints[nz][i].diameter)) {
                nextBasePoint = keyPoints[nz][i];
                mask[nz][i] = 1;
                outPoints.push_back(nextBasePoint);
                nMatched++;
            }
        }
        if (nMatched == 0) {
            break;
        }
    }
}

void MeBlobDector::computeBlob3DPos(short *pData, int dims[], double spacing[], MeBlobParams &blobParams,
                                     vector<Point3f> &resultPoint)
{
    int rows = dims[1];
    int cols = dims[0];
    
    short sThreshold = blobParams.threshold;
    int winWidth = blobParams.window; // default
    int winMin = blobParams.level - winWidth/2;

    // 1. 识别每一个层图像的圆
    std::vector<std::vector<MeKeyPoint>> keyPoints(dims[2]);
    for (int z =0;z<dims[2]; z++) {
        cv::Mat img1 = cv::Mat(rows, cols, CV_8U);
        cv::Mat gray = cv::Mat(rows, cols, CV_8U);
        for (int y = 0; y < dims[1]; y++) {
            char *ptr = img1.ptr<char>(y);
            char *ptrGray = gray.ptr<char>(y);
            for (int x = 0; x < dims[0]; x++) {
                short vPixel = pData[dims[0]*dims[1]*z + dims[0] *y +x];
                if(sThreshold < vPixel) {
                    ptr[x] = 255;
                } else { 
                    ptr[x] = 0;
                } 
                
                if (vPixel >= winMin + winWidth) {
                    ptrGray[x] = 255;
                } else if (vPixel <= winMin){
                    ptrGray[x] = 0;
                } else {
                    ptrGray[x] = (vPixel - winMin)*255/winWidth;
                }
            }
        }

        fastBlob2DDetect(gray, img1, blobParams, z, keyPoints[z]);
       
#ifdef DEUBG_BLOB
        if (keyPoints[z].size() > 0) {
            cout << "z: " << z << endl;
            drawKeyPointsWithCircle(gray, keyPoints[z]);
        }
#endif
    }
    
    // 2. 在z方向上，把这些点关联起来
    vector<vector<MeKeyPoint>> featureKeyPoints;
    vector<vector<char>> mask(dims[2]); // 标记遍历过的点
    for (int z = 0; z < (dims[2]); z++) {
        for (int i = 0; i< keyPoints[z].size(); i++) {
            mask[z].push_back(0);
        }
    }
    // 判断靠近点
    for (int z = 0; z < (dims[2]-1); z++) {
        // 下面一层寻找当前的
        int nz = z+1;
        for (int i = 0; i < keyPoints[z].size(); i++) {
            if (mask[z][i] == 1) {
                continue;
            }
            
            vector<MeKeyPoint> outPoints;
            mask[z][i] = 1;
            travelUp( keyPoints[z][i], keyPoints, mask, nz, dims[2], outPoints);
            if (outPoints.size() > 3) {
                featureKeyPoints.push_back(outPoints);
            }
        }
    }
    
    // 3. 计算最后结果
    float cx = 0, cy = 0, cz = 0;
    for (int i = 0; i< featureKeyPoints.size(); i++) {
#ifdef PRINT_TRACE
        cout << "result_" << i << ":" << endl;
#endif
        // todo: 简单计算
        cz = featureKeyPoints[i][0].z + (featureKeyPoints[i].size() - 1)/2.0;
        int n = cvRound((featureKeyPoints[i].size() - 1)/2.0);
        cx = (featureKeyPoints[i][n-1].pt.x + featureKeyPoints[i][n].pt.x + featureKeyPoints[i][n+1].pt.x)/3.0;
        cy = (featureKeyPoints[i][n-1].pt.y + featureKeyPoints[i][n].pt.y + featureKeyPoints[i][n+1].pt.y)/3.0;
        resultPoint.push_back(Point3f(cx*spacing[0],cy*spacing[1],cz*spacing[2]));
#ifdef PRINT_TRACE
        for (int j =0; j < featureKeyPoints[i].size(); j++) {
            
            cout << featureKeyPoints[i][j].pt << "," << featureKeyPoints[i][j].z << "; " << endl;
            
        }
#endif
    }
}
